using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace CreamX.Common
{
    public static class VectorUtil
    {

        public static float CrossPod(Vector2 L, Vector2 R)
        {
            //Vector3 cross = Vector3.Cross(new Vector3(L, 0), new Vector3(R, 0));
            return (L.X * R.Y - L.Y * R.X);

        }
        public static Vector2 VectorToLocalSpace(Vector2 vec,
                                     Vector2 AgentHeading,
                                     Vector2 AgentSide)
        {




            Vector2 TransVec = vec;
            float desiredAngle1 = (float)Math.Atan2(AgentHeading.Y, AgentHeading.X);
            if (desiredAngle1 < 0)
                desiredAngle1 += MathHelper.TwoPi;
            TransVec = Vector2.Transform(vec, Matrix.CreateRotationZ(desiredAngle1 * -1));


            return TransVec;

        }

        public static void VectorToLocalSpace(ref Vector2 vec,
                                     ref Vector2 AgentHeading,
                                     ref Vector2 AgentSide,
                                    out Vector2 position)
        {




            Vector2 TransVec = vec;
            float desiredAngle1 = (float)Math.Atan2(AgentHeading.Y, AgentHeading.X);
            if (desiredAngle1 < 0)
                desiredAngle1 += MathHelper.TwoPi;

            Matrix __m = Matrix.CreateRotationZ(desiredAngle1 * -1);
            Vector2.Transform(ref TransVec, ref __m, out TransVec);
            position = TransVec;

        }

        public static void PointToLocalSpace(ref Vector2 point,
                             ref Vector2 AgentHeading,
                             ref Vector2 AgentSide,
                              ref Vector2 AgentPosition,
                                out Vector2 position)
        {

            //make a copy of the point
            Vector2 TransPoint = point;
            float desiredAngle1 = (float)Math.Atan2(AgentHeading.Y, AgentHeading.X);
            if (desiredAngle1 < 0)
                desiredAngle1 += MathHelper.TwoPi;
            Vector2 __pos = point - AgentPosition;
            Matrix __mat = Matrix.CreateRotationZ(desiredAngle1 * -1);
            Vector2.Transform(ref __pos, ref __mat, out TransPoint);

            position = TransPoint;
        }

        public static Vector2 PointToLocalSpace(Vector2 point,
                             Vector2 AgentHeading,
                             Vector2 AgentSide,
                             Vector2 AgentPosition)
        {

            //make a copy of the point
            Vector2 TransPoint = point;
            float desiredAngle1 = (float)Math.Atan2(AgentHeading.Y, AgentHeading.X);
            if (desiredAngle1 < 0)
                desiredAngle1 += MathHelper.TwoPi;
            Vector2 __pos = point - AgentPosition;
            Matrix __mat = Matrix.CreateRotationZ(desiredAngle1 * -1);
            TransPoint = Vector2.Transform(__pos, __mat);

            return TransPoint;
        }


        public static Vector2 PointToWorldSpace(Vector2 point,
                                      Vector2 AgentHeading,
                                      Vector2 AgentSide,
                                      Vector2 AgentPosition)
        {

            //Vector3 vec=Vector2.Transform(point,Matrix.CreateTranslation(new Vector3(AgentPosition,0)) * Matrix.CreateRotationZ(
            //make a copy of the point
            Vector2 TransPoint = point;
            float desiredAngle1 = (float)Math.Atan2(AgentHeading.Y, AgentHeading.X);
            
            if (desiredAngle1 < 0)
                desiredAngle1 += MathHelper.TwoPi;
            if (desiredAngle1 > MathHelper.TwoPi)
                desiredAngle1 -= MathHelper.TwoPi;
            
            TransPoint = Vector2.Transform(point,
                                 Matrix.CreateRotationZ(desiredAngle1)*Matrix.CreateTranslation(new Vector3(AgentPosition, 0))
                                            );

            return TransPoint;

        }

        public static Vector2 VectorToWorldSpace(Vector2 vec,
                                             Vector2 AgentHeading,
                                             Vector2 AgentSide)
        {

            Vector2 TransPoint = vec;
            float desiredAngle1 = (float)Math.Atan2(AgentHeading.Y, AgentHeading.X);
            float f = MathHelper.ToDegrees(desiredAngle1);
            if (desiredAngle1 < 0)
                desiredAngle1 += MathHelper.TwoPi;
            if (desiredAngle1 > MathHelper.TwoPi)
                desiredAngle1 -= MathHelper.TwoPi;
            TransPoint = Vector2.Transform(vec, Matrix.CreateRotationZ(desiredAngle1));


            return TransPoint;


        }
        public static void VectorToWorldSpace(ref Vector2 vec,
                                                     ref Vector2 AgentHeading,
                                                     ref Vector2 AgentSide,
            out Vector2 position)
        {

            Vector2 TransPoint = vec;
            float desiredAngle1 = (float)Math.Atan2(AgentHeading.Y, AgentHeading.X);
            if (desiredAngle1 < 0)
                desiredAngle1 += MathHelper.TwoPi;
            if (desiredAngle1 > MathHelper.TwoPi)
                desiredAngle1 -= MathHelper.TwoPi;
            Matrix __m = Matrix.CreateRotationZ(desiredAngle1);
            Vector2.Transform(ref TransPoint, ref __m, out position);

        }


        private static void RotateMatrix(ref Matrix matrix, Vector2 fwd, Vector2 side)
        {

            Matrix mat = Matrix.Identity;
            mat.M11 = fwd.X; mat.M12 = fwd.Y; mat.M13 = 0;
            mat.M21 = side.X; mat.M22 = side.Y; mat.M23 = 0;
            mat.M31 = 0; mat.M32 = 0; mat.M33 = 1;

            //and multiply
            matrix *= mat;
        }

        public static float TurnToFace(Vector2 position, Vector2 faceThis,
            float currentAngle, float turnSpeed)
        {
            // consider this diagram:
            //         B 
            //        /|
            //      /  |
            //    /    | y
            //  / o    |
            // A--------
            //     x
            // 
            // where A is the position of the object, B is the position of the target,
            // and "o" is the angle that the object should be facing in order to 
            // point at the target. we need to know what o is. using trig, we know that
            //      tan(theta)       = opposite / adjacent
            //      tan(o)           = y / x
            // if we take the arctan of both sides of this equation...
            //      arctan( tan(o) ) = arctan( y / x )
            //      o                = arctan( y / x )
            // so, we can use x and y to find o, our "desiredAngle."
            // x and y are just the differences in position between the two objects.
            float x = faceThis.X - position.X;
            float y = faceThis.Y - position.Y;

            // we'll use the Atan2 function. Atan will calculates the arc tangent of 
            // y / x for us, and has the added benefit that it will use the signs of x
            // and y to determine what cartesian quadrant to put the result in.
            // http://msdn2.microsoft.com/en-us/library/system.math.atan2.aspx
            float desiredAngle = (float)Math.Atan2(y, x);

            // so now we know where we WANT to be facing, and where we ARE facing...
            // if we weren't constrained by turnSpeed, this would be easy: we'd just 
            // return desiredAngle.
            // instead, we have to calculate how much we WANT to turn, and then make
            // sure that's not more than turnSpeed.

            // first, figure out how much we want to turn, using WrapAngle to get our
            // result from -Pi to Pi ( -180 degrees to 180 degrees )
            float difference = WrapAngle(desiredAngle - currentAngle);

            // clamp that between -turnSpeed and turnSpeed.
            difference = MathHelper.Clamp(difference, -turnSpeed, turnSpeed);

            // so, the closest we can get to our target is currentAngle + difference.
            // return that, using WrapAngle again.
            return WrapAngle(currentAngle + difference);
        }

        /// <summary>
        /// Returns the angle expressed in radians between -Pi and Pi.
        /// <param name="radians">the angle to wrap, in radians.</param>
        /// <returns>the input value expressed in radians from -Pi to Pi.</returns>
        /// </summary>
        private static float WrapAngle(float radians)
        {
            while (radians < -MathHelper.Pi)
            {
                radians += MathHelper.TwoPi;
            }
            while (radians > MathHelper.Pi)
            {
                radians -= MathHelper.TwoPi;
            }
            return radians;
        }

        public static Vector2 GetSide(Vector2 vector2)
        {
            return new Vector2(-vector2.Y, vector2.X);
        }
    }
}
